/* 
 * File:   algorithmControl.cpp
 * Author: robotic
 * 
 * Created on 26 de Julho de 2012, 14:30
 */

#include "controlEpuck/algorithmControl.hpp"

AlgorithmControl::AlgorithmControl() {

    this->lonQuad = 0.3;
    this-> k = 0;
    this->numMaxRobots = NUM_MAX_ROBOTS;
    this->il = 0;
    
    char id[] = {'1','2','3','4','5','6'};

    for(int i=0;i< NUM_MAX_ROBOTS;i++){

        epucksSetOdom[i]+="epuckSetOdom";
        epucksSetOdom[i]+=id[i]; 
        epucksGetOdom[i]+="epuckGetOdom";
        epucksGetOdom[i]+=id[i]; 
        epucksSetVel[i]+="epuckVelocity";
        epucksSetVel[i]+=id[i]; 
    
        cliGetOdom[i] = nControl.serviceClient<epuckDriver::GetOdometry>(epucksGetOdom[i].c_str());
        cliSetOdom[i] = nControl.serviceClient<epuckDriver::SetOdometry>(epucksSetOdom[i].c_str());
        pub[i] = nControl.advertise<geometry_msgs::Twist>(epucksSetVel[i].c_str(),1);
    }
    
    request[0].request.pose2d.x = 0*lonQuad;
    request[0].request.pose2d.y = 2*lonQuad;
    request[0].request.pose2d.theta = 0;

    request[1].request.pose2d.x = 0*lonQuad;
    request[1].request.pose2d.y = 3*lonQuad;
    request[1].request.pose2d.theta = 0;
    
    request[2].request.pose2d.x = 0*lonQuad;
    request[2].request.pose2d.y = 1*lonQuad;
    request[2].request.pose2d.theta = 0;
    
    request[3].request.pose2d.x = 0*lonQuad;
    request[3].request.pose2d.y = 7*lonQuad;
    request[3].request.pose2d.theta = 0;
    
    request[4].request.pose2d.x = 0*lonQuad;
    request[4].request.pose2d.y = 6*lonQuad;
    request[4].request.pose2d.theta = 0;
    
    request[5].request.pose2d.x = 0*lonQuad;
    request[5].request.pose2d.y = 5*lonQuad;
    request[5].request.pose2d.theta = 0;

    this->A = gsl_matrix_calloc(4, 4);
    this->B = gsl_matrix_calloc(4, 2);

    // Initializing A
    gsl_matrix_set(this->A, 0, 1, 1);
    gsl_matrix_set(this->A, 1, 3, -k);
    gsl_matrix_set(this->A, 2, 3, 1);
    gsl_matrix_set(this->A, 3, 1, k);

    // Initializing B
    gsl_matrix_set(this->B, 1, 0, 1);
    gsl_matrix_set(this->B, 3, 1, 1);

    this->vetCot = gsl_vector_calloc(6);
    this->y0 = gsl_vector_calloc(24);

    //y11
    gsl_vector_set(this->y0, 0, 0 * this->lonQuad);
    gsl_vector_set(this->y0, 1, 0.0707);
    gsl_vector_set(this->y0, 2, 2 * this->lonQuad);
    gsl_vector_set(this->y0, 3, 0.0707);
    //y21:	
    gsl_vector_set(this->y0, 4, 0 * this->lonQuad);
    gsl_vector_set(this->y0, 5, 0.0707);
    gsl_vector_set(this->y0, 6, 3 * this->lonQuad);
    gsl_vector_set(this->y0, 7, 0.0707);
    //y31:
    gsl_vector_set(this->y0, 8, 0 * this->lonQuad);
    gsl_vector_set(this->y0, 9, 0.0707);
    gsl_vector_set(this->y0, 10, 1 * this->lonQuad);
    gsl_vector_set(this->y0, 11, 0.0707);
    //y41:
    gsl_vector_set(this->y0, 12, 0 * this->lonQuad);
    gsl_vector_set(this->y0, 13, 0.0707);
    gsl_vector_set(this->y0, 14, 7 * this->lonQuad);
    gsl_vector_set(this->y0, 15, 0.0707);
    //y51:
    gsl_vector_set(this->y0, 16, 0 * this->lonQuad);
    gsl_vector_set(this->y0, 17, 0.0707);
    gsl_vector_set(this->y0, 18, 6 * this->lonQuad);
    gsl_vector_set(this->y0, 19, 0.0707);
    //y61:
    gsl_vector_set(this->y0, 20, 0 * this->lonQuad);
    gsl_vector_set(this->y0, 21, 0.0707);
    gsl_vector_set(this->y0, 22, 5 * this->lonQuad);
    gsl_vector_set(this->y0, 23, 0.0707);

    this->hp = gsl_matrix_calloc(12, 1);
    gsl_matrix_set(this->hp, 0, 0, 2 * this->lonQuad);
    gsl_matrix_set(this->hp, 1, 0, 0);
    gsl_matrix_set(this->hp, 2, 0, 1 * this->lonQuad);
    gsl_matrix_set(this->hp, 3, 0, 0.5 * this->lonQuad);
    gsl_matrix_set(this->hp, 4, 0, 1 * this->lonQuad);
    gsl_matrix_set(this->hp, 5, 0, -0.5 * this->lonQuad);
    gsl_matrix_set(this->hp, 6, 0, 0 * this->lonQuad);
    gsl_matrix_set(this->hp, 7, 0, 1.5 * this->lonQuad);
    gsl_matrix_set(this->hp, 8, 0, 0 * this->lonQuad);
    gsl_matrix_set(this->hp, 9, 0, 0 * this->lonQuad);
    gsl_matrix_set(this->hp, 10, 0, 0 * this->lonQuad);
    gsl_matrix_set(this->hp, 11, 0, -1.5 * this->lonQuad);

    this->temp = gsl_matrix_calloc(2, 1);
    gsl_matrix_set(temp, 0, 0, 1);
    gsl_matrix_set(temp, 1, 0, 0);

    this->h = gsl_matrix_calloc(24, 1);
    kron(h, hp, temp);

    this->hv = gsl_vector_calloc(this->h->size1);
    gsl_matrix_get_col(this->hv, this->h, 0);

    this->dimAdj = alter_dim(1);
    this->Adj = gsl_matrix_calloc(dimAdj, dimAdj);
    alter_set(Adj, 1);

    this->D = gsl_matrix_calloc(dimAdj, dimAdj);
    double sum;
    for (int i = 0; i < dimAdj; i++) {
        sum = 0;
        for (int j = 0; j < dimAdj; j++) {
            sum = sum + gsl_matrix_get(Adj, i, j);
        }
        gsl_matrix_set(D, i, i, sum);
    }

    this->L = gsl_matrix_alloc(this->dimAdj, this->dimAdj);
    this->Leig = gsl_matrix_alloc(this->dimAdj, this->dimAdj);

    gsl_matrix_memcpy(this->L, this->D);
    gsl_matrix_sub(this->L, this->Adj);

    gsl_matrix_memcpy(this->Leig, this->L);
    this->eigL1 = gsl_vector_complex_calloc(this->dimAdj);
    this->w = gsl_eigen_nonsymm_alloc(this->dimAdj);
    gsl_eigen_nonsymm_params(0, 1, this->w);
    gsl_eigen_nonsymm(this->Leig, this->eigL1, this->w);
    gsl_eigen_nonsymm_free(this->w);

    this->F = gsl_matrix_calloc(2, 4);
    gsl_matrix_set(this->F, 0, 0, -0.00855);
    gsl_matrix_set(this->F, 0, 1, -0.12);
    gsl_matrix_set(this->F, 1, 2, -0.00855);
    gsl_matrix_set(this->F, 1, 3, -0.12);

    Ae = gsl_matrix_calloc(this->numMaxRobots * 4, this->numMaxRobots * 4);
    kron_eye_with_matrix(this->Ae, this->A, this->numMaxRobots);
    //Be = kron(eye(N),B);
    this->Be = gsl_matrix_calloc(this->numMaxRobots * 4, this->numMaxRobots * 2);
    kron_eye_with_matrix(this->Be, this->B, this->numMaxRobots);
    //Fe = kron(eye(N),F);
    this->Fe = gsl_matrix_calloc(this->numMaxRobots * 2, this->numMaxRobots * 4);
    kron_eye_with_matrix(this->Fe, this->F, this->numMaxRobots);
    //Le = kron(L,eye(4));
    this->Le = gsl_matrix_calloc(4 * this->dimAdj, 4 * this->dimAdj);
    kron_matrix_with_eye(this->Le, this->L, 4);

    this->Temp1 = gsl_matrix_calloc(2 * this->numMaxRobots, 4 * this->numMaxRobots);
    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, this->Fe, this->Le, 0.0, this->Temp1);
    // Temp2 = Be*Temp1
    this->Temp2 = gsl_matrix_calloc(4 * this->numMaxRobots, 4 * this->numMaxRobots);
    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, this->Be, this->Temp1, 0.0, this->Temp2);
    // Temp2 = Ae + Temp2
    gsl_matrix_add(this->Temp2, this->Ae);

    this->Form1 = gsl_matrix_alloc(4 * this->numMaxRobots, 4 * this->numMaxRobots);
    gsl_matrix_memcpy(this->Form1, this->Temp2);

    this->eigForm1 = gsl_vector_complex_calloc(4 * this->numMaxRobots);

    w = gsl_eigen_nonsymm_alloc(4 * this->numMaxRobots);
    gsl_eigen_nonsymm_params(0, 1, this->w);
    gsl_eigen_nonsymm(this->Form1, this->eigForm1, this->w);
    gsl_eigen_nonsymm_free(this->w);

    this->sample = 0.44; //0.20 + 0.24;		// Sample time for 6 e-pucks
    this->tFinalMax = 160.0;
    this->tFinal = 70000.0; // t_final em (ms)
    this->numIter = tFinalMax / sample;

    this->y = gsl_matrix_calloc(this->numMaxRobots * 4, this->numIter + 1);
    this->dy = gsl_matrix_calloc(this->numMaxRobots * 4, this->numIter + 1);
    this->yv = gsl_vector_calloc(this->numMaxRobots * 4);
    this->tempv = gsl_vector_calloc(this->numMaxRobots * 4);
    this->tempv1 = gsl_vector_calloc(this->numMaxRobots * 4);
    this->tempv2 = gsl_vector_calloc(this->numMaxRobots * 4);
    this->tempvM = gsl_vector_calloc(this->numMaxRobots * 2);
    this->tempvM1 = gsl_vector_calloc(this->numMaxRobots * 2);
    this->pos = gsl_matrix_calloc(this->numMaxRobots * 4, this->numIter + 1);
    this->posi = gsl_vector_calloc(this->numMaxRobots * 4);

    gsl_blas_dcopy(y0, yv);

    this->posLocalization = gsl_matrix_calloc(this->numIter, this->numMaxRobots * 3);
}

void AlgorithmControl::control() {

    if (this->allDetected) {
        gsl_matrix_set_col(this->dy, this->loopControl, this->tempv1);
        gsl_matrix_set_col(this->y, this->loopControl, this->yv);

        if (this->loopControl == this->numIter / 6) {
            this->dimAdj = alter_dim(2);
            this->Adj = gsl_matrix_calloc(this->dimAdj, this->dimAdj);
            alter_set(this->Adj, 2);
            printf("Leader 2\n");
        }
        if (this->loopControl == this->numIter / 3) {
            this->dimAdj = alter_dim(4);
            this->Adj = gsl_matrix_calloc(this->dimAdj, this->dimAdj);
            alter_set(this->Adj, 4);
            printf("Leader 4\n");
        }
        if (this->loopControl == this->numIter / 2) {
            this->dimAdj = alter_dim(6);
            this->Adj = gsl_matrix_calloc(this->dimAdj, this->dimAdj);
            alter_set(this->Adj, 6);
            printf("Leader 6\n");
        }
        if (this->loopControl == 2 * this->numIter / 3) {
            this->dimAdj = alter_dim(5);
            this->Adj = gsl_matrix_calloc(this->dimAdj, this->dimAdj);
            alter_set(this->Adj, 5);
            printf("Leader 5\n");
        }
        if (this->loopControl == 5 * this->numIter / 6) {
            this->dimAdj = alter_dim(3);
            this->Adj = gsl_matrix_calloc(this->dimAdj, this->dimAdj);
            alter_set(this->Adj, 3);
            printf("Leader 3\n");
        }

        ///////////////// Matriz Diagonal         (Vide Teoria dos Grafos)
        // diagonal=sum(Adj');
        // D=diag(diagonal);
        double sum;
        for (int j = 0; j < this->dimAdj; j++) {
            sum = 0;
            for (int j2 = 0; j2 < this->dimAdj; j2++) {
                sum = sum + gsl_matrix_get(this->Adj, j, j2);
            }
            gsl_matrix_set(this->D, j, j, sum);
        }
        ///////////////////

        // Directed Laplacian (see Veerman 2004)
        // L=D-Adj;
        gsl_matrix_memcpy(this->L, this->D);
        gsl_matrix_sub(this->L, this->Adj);

        //Le = kron(L,eye(4));
        kron_matrix_with_eye(this->Le, this->L, 4);

        ////////// Dy(:,i+1)=Ae*y(:,i)+Be*Fe*Le*(y(:,i)-h); 
        // tempv = yv
        gsl_blas_dcopy(this->yv, this->tempv);

        // tempv = tempv-hv
        gsl_blas_daxpy(-1.0, this->hv, this->tempv);
        // tempv1 = Le*tempv
        gsl_blas_dgemv(CblasNoTrans, 1.0, this->Le, this->tempv, 0.0, this->tempv1);

        // tempv_m = Fe*tempv1
        gsl_blas_dgemv(CblasNoTrans, 1.0, this->Fe, this->tempv1, 0.0, this->tempvM);
        // tempv = Be*tempv_mxerro_v(1,2995:3000)
        gsl_blas_dgemv(CblasNoTrans, 1.0, this->Be, this->tempvM, 0.0, this->tempv);
        // tempv2 = yv
        gsl_blas_dcopy(this->yv, this->tempv2);
        // tempv1 = Ae*tempv2
        gsl_blas_dgemv(CblasNoTrans, 1.0, this->Ae, this->tempv2, 0.0, this->tempv1);
        // tempv1 = tempv1 + tempv
        gsl_blas_daxpy(1.0, this->tempv, this->tempv1);
        // dy = tempv1
        ///////////////

        gsl_blas_daxpy(this->sample, this->tempv1, this->yv);

        gsl_matrix_set_col(this->pos, loopControl, this->posi);

        double x_r, y_r, vX_r, vY_r, vX1_r, vY1_r, aX_r, aY_r;
        double v_ref, w_ref, xi, yi, thetai, xerror, yerror, thetaerror, vel_v, vel_w;
        double xi_odo[6], yi_odo[6], thetai_odo[6];

        for (int j = 0; j < this->numMaxRobots; j++) {

            x_r = gsl_vector_get(this->tempv2, (0 + j * 4));
            y_r = gsl_vector_get(this->tempv2, (2 + j * 4));
            vX_r = gsl_vector_get(this->tempv2, (1 + j * 4));
            vY_r = gsl_vector_get(this->tempv2, (3 + j * 4));
            vX1_r = gsl_vector_get(this->yv, (1 + j * 4));
            vY1_r = gsl_vector_get(this->yv, (3 + j * 4));
            aX_r = (vX1_r - vX_r) / this->sample;
            aY_r = (vY1_r - vY_r) / this->sample;

            v_ref = sqrt(vX_r * vX_r + vY_r * vY_r);
            w_ref = (vX_r * aY_r - vY_r * aX_r) / (v_ref * v_ref);

            // Camera PtGrey
            if (robots[j].find) {

                cliGetOdom[j].call(response[j]);       
    
                x_r = gsl_vector_get(tempv2,(0+j*4));
			    y_r = gsl_vector_get(tempv2,(2+j*4));
			    vX_r = gsl_vector_get(tempv2,(1+j*4));
			    vY_r = gsl_vector_get(tempv2,(3+j*4));
			    vX1_r = gsl_vector_get(yv,(1+j*4));
			    vY1_r = gsl_vector_get(yv,(3+j*4));
			    aX_r = (vX1_r-vX_r)/sample;
			    aY_r = (vY1_r-vY_r)/sample;

                xi = gsl_matrix_get(posLocalization, loopControl, 0 + j * 3);
                yi = gsl_matrix_get(posLocalization, loopControl, 1 + j * 3);
                thetai = gsl_matrix_get(posLocalization, loopControl, 2 + j * 3);

                xi = -gsl_matrix_get(posLocalization, loopControl, 1 + j * 3) + 3.0;
                yi = gsl_matrix_get(posLocalization, loopControl, 0 + j * 3) - 0.9;
                thetai = -gsl_matrix_get(posLocalization, loopControl, 2 + j * 3);

            } else {
                if (loopControl > 1) {

                    //Odometria E - puck
                    xi = gsl_matrix_get(pos, loopControl - 1, 0 + 3 * j) + (robots[j].x - xi_odo[j]);
                    yi = gsl_matrix_get(pos, loopControl - 1, 1 + 3 * j) + (robots[j].y - yi_odo[j]);
                    thetai = gsl_matrix_get(pos, loopControl - 1, 2 + 3 * j) + (robots[j].theta - thetai_odo[j]);

                } else {
                    xi = response[j].response.pose2d.x;
                    yi = response[j].response.pose2d.y;
                    thetai = response[j].response.pose2d.theta;
                }
            }

            //Odometria E-puck
            xi = response[j].response.pose2d.x;
            yi = response[j].response.pose2d.y;
            thetai = response[j].response.pose2d.theta;
        
            pose2d[j] = response[j].response.pose2d;     

            gsl_vector_set(this->posi, 0 + 3 * j, xi);
            gsl_vector_set(this->posi, 1 + 3 * j, yi);
            gsl_vector_set(this->posi, 2 + 3 * j, thetai);

            xerror = (x_r - xi) * cos(thetai) + (y_r - yi) * sin(thetai);
            yerror = -(x_r - xi) * sin(thetai) + (y_r - yi) * cos(thetai);
            thetaerror = atan2(vY_r, vX_r)-(thetai);

            if (thetaerror > 2 * 3.14159265)
                thetaerror = thetaerror - 2 * 3.14159265;

            vel_v = v_ref * cos(thetaerror) + 1.5 * xerror;
            vel_w = w_ref + v_ref * (20 * yerror + 10 * sin(thetaerror));
            
            twist.linear.x = vel_v;
            twist.angular.z = vel_w;            

            pub[j].publish(twist);
        }

        ++this->loopControl;
    }
    ++this->il;
}

//Realizar a parte de comunicacao entre a camera e o controle
//Criar um nó comum e a comunicacao entre ambos
//Precisa passar posicao Pose2D
void AlgorithmControl::camControlCallBack(camControl::CameraControlMsg msg){

    
    int numRobots = 0;

    for(int r=0;r<NUM_MAX_ROBOTS;r++)
	{
		// Only update the robot position if robot could be found on the image
		if( msg.find[r] )
		{
			pose2d[r] = msg.pose2d[r];
            numRobots++;			
			//printf("robot:%d , x=%f, y=%f , theta=%f\n",r+1,robot_x[r],robot_y[r],robot_theta[r]);
		}
		
		// Log this
		gsl_matrix_set(posLocalization,this->il,3*r+0,pose2d[r].x); // x
		gsl_matrix_set(posLocalization,this->il,3*r+1,pose2d[r].y); // y
		gsl_matrix_set(posLocalization,this->il,3*r+2,pose2d[r].theta); // theta
		
	}
    
    if(numRobots == NUM_MAX_ROBOTS)
		this->allDetected = true;
    
    this->control();
}

void AlgorithmControl::kron(gsl_matrix *K, gsl_matrix *A, gsl_matrix *B) {

    int ia, ja, ib, jb;
    int An, Am, Bn, Bm;
    double temp;
    An = A->size1;
    Am = A->size2;
    Bn = B->size1;
    Bm = B->size2;
    for (ia = 0; ia < An; ia++) {
        for (ja = 0; ja < Am; ja++) {
            for (ib = 0; ib < Bn; ib++) {
                for (jb = 0; jb < Bm; jb++) {
                    temp = gsl_matrix_get(A, ia, ja) * gsl_matrix_get(B, ib, jb);
                    gsl_matrix_set(K, ib + ia*Bn, jb + ja*Bm, temp);
                }
            }
        }
    }
}

void AlgorithmControl::kron_matrix_with_eye(gsl_matrix * K, gsl_matrix * A, int sizeEye) {
    int n, m, i, j, k;
    int An, Am;
    An = A->size1;
    Am = A->size2;
    n = An*sizeEye;
    m = Am*sizeEye;
    //K = gsl_matrix_calloc(n,m);
    for (i = 0; i < An; i = i++) {
        for (j = 0; j < Am; j = j++) {
            for (k = 0; k < sizeEye; k++) {
                gsl_matrix_set(K, i * sizeEye + k, j * sizeEye + k, gsl_matrix_get(A, i, j));
            }
        }
    }
}

void AlgorithmControl::kron_eye_with_matrix(gsl_matrix * K, gsl_matrix * A, int sizeEye) {
    int n, m, i, j, k;
    int An, Am;
    An = A->size1;
    Am = A->size2;
    n = An*sizeEye;
    m = Am*sizeEye;
    //K= gsl_matrix_calloc(n,m);
    for (k = 0; k < sizeEye; k++) {
        for (i = 0; i < An; i = i + 1) {
            for (j = 0; j < Am; j = j + 1) {
                gsl_matrix_set(K, i + k*An, j + k*Am, gsl_matrix_get(A, i, j));
            }
        }
    }
}

int AlgorithmControl::alter_dim(int n) {
    int d = 0;
    switch (n) {
        case 1:
            d = 6;
            break;
        case 2:
            d = 6;
            break;
        case 3:
            d = 6;
            break;
        case 4:
            d = 6;
            break;
        case 5:
            d = 6;
            break;
        case 6:
            d = 6;
            break;
        case 30:
            d = 5;
        case 60:
            d = 5;
        default:
            d = 0;
            break;
    }

    return d;

}

void AlgorithmControl::alter_set(gsl_matrix *A, int n) {
    switch (n) {
        case 1:
            gsl_matrix_set(A, 1, 0, 1);
            gsl_matrix_set(A, 2, 0, 1);
            gsl_matrix_set(A, 2, 1, 1);
            gsl_matrix_set(A, 3, 1, 1);
            gsl_matrix_set(A, 3, 5, 1);
            gsl_matrix_set(A, 4, 2, 1);
            gsl_matrix_set(A, 4, 5, 1);
            gsl_matrix_set(A, 5, 1, 1);
            gsl_matrix_set(A, 5, 2, 1);
            break;
        case 2:
            gsl_matrix_set(A, 0, 1, 1);
            gsl_matrix_set(A, 2, 0, 1);
            gsl_matrix_set(A, 2, 1, 1);
            gsl_matrix_set(A, 3, 1, 1);
            gsl_matrix_set(A, 3, 5, 1);
            gsl_matrix_set(A, 4, 5, 1);
            gsl_matrix_set(A, 5, 1, 1);
            gsl_matrix_set(A, 5, 2, 1);
            break;
        case 3:
            gsl_matrix_set(A, 0, 2, 1);
            gsl_matrix_set(A, 1, 0, 1);
            gsl_matrix_set(A, 1, 2, 1);
            gsl_matrix_set(A, 3, 1, 1);
            gsl_matrix_set(A, 3, 5, 1);
            gsl_matrix_set(A, 4, 2, 1);
            gsl_matrix_set(A, 4, 5, 1);
            gsl_matrix_set(A, 5, 2, 1);
            break;
        case 4:
            gsl_matrix_set(A, 0, 1, 1);
            gsl_matrix_set(A, 0, 2, 1);
            gsl_matrix_set(A, 1, 3, 1);
            gsl_matrix_set(A, 2, 1, 1);
            gsl_matrix_set(A, 4, 2, 1);
            gsl_matrix_set(A, 4, 5, 1);
            gsl_matrix_set(A, 5, 2, 1);
            gsl_matrix_set(A, 5, 3, 1);
            break;
        case 5:
            gsl_matrix_set(A, 0, 1, 1);
            gsl_matrix_set(A, 1, 2, 1);
            gsl_matrix_set(A, 1, 5, 1);
            gsl_matrix_set(A, 2, 4, 1);
            gsl_matrix_set(A, 3, 1, 1);
            gsl_matrix_set(A, 3, 5, 1);
            gsl_matrix_set(A, 5, 4, 1);
            break;
        case 6:
            gsl_matrix_set(A, 0, 1, 1);
            gsl_matrix_set(A, 1, 2, 1);
            gsl_matrix_set(A, 1, 5, 1);
            gsl_matrix_set(A, 2, 0, 1);
            gsl_matrix_set(A, 2, 5, 1);
            gsl_matrix_set(A, 3, 2, 1);
            gsl_matrix_set(A, 3, 5, 1);
            gsl_matrix_set(A, 4, 2, 1);
            gsl_matrix_set(A, 4, 5, 1);
            break;
        case 30:
            gsl_matrix_set(A, 1, 0, 1);
            gsl_matrix_set(A, 2, 1, 1);
            gsl_matrix_set(A, 2, 4, 1);
            gsl_matrix_set(A, 3, 0, 1);
            gsl_matrix_set(A, 3, 1, 1);
            gsl_matrix_set(A, 4, 1, 1);
            gsl_matrix_set(A, 4, 3, 1);
        case 60:
            gsl_matrix_set(A, 1, 0, 1);
            gsl_matrix_set(A, 2, 0, 1);
            gsl_matrix_set(A, 2, 1, 1);
            gsl_matrix_set(A, 3, 1, 1);
            gsl_matrix_set(A, 3, 2, 1);
            gsl_matrix_set(A, 4, 2, 1);
            gsl_matrix_set(A, 4, 3, 1);
        default:
            break;
    }
}

